{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 02-01 模型的导入和查看\n",
    "\n",
    "\n",
    "\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "\n",
    "### laikago模型\n",
    "\n",
    "宇树机器人（unitree robotics）在github上的官方仓库中，提供了unitree_pybullet包。\n",
    "\n",
    "其中给出了laikago、aliengo、a1三个机器人的模型。\n",
    "这里选择laikago的模型，将其拷贝到models文件夹中使用。\n",
    "\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 使用MeshCat显示laikago的三维模型\n",
    "\n",
    "使用可视化工具可直观查看模型。\n",
    "这里用MeshCat。因为Drake的很多例子中都是用MeshCat。\n",
    "\n",
    "\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "server_args = []\n",
    "\n",
    "# Start a single meshcat server instance to use for the remainder of this notebook.\n",
    "from meshcat.servers.zmqserver import start_zmq_server_as_subprocess\n",
    "proc, zmq_url, web_url = start_zmq_server_as_subprocess(server_args=server_args)\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Determine if this notebook is currently running as a notebook or a unit test.\n",
    "from IPython import get_ipython\n",
    "running_as_notebook = get_ipython() and hasattr(get_ipython(), 'kernel')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Imports\n",
    "import numpy as np\n",
    "import pydot\n",
    "from ipywidgets import Dropdown, Layout\n",
    "from IPython.display import display, HTML, SVG\n",
    "\n",
    "from pydrake.all import (\n",
    "    AddMultibodyPlantSceneGraph, ConnectMeshcatVisualizer, DiagramBuilder, \n",
    "    FindResourceOrThrow, GenerateHtml, InverseDynamicsController, \n",
    "    MultibodyPlant, Parser, Simulator)\n",
    "from pydrake.multibody.jupyter_widgets import MakeJointSlidersThatPublishOnCallback"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 12,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Connecting to meshcat-server at zmq_url=tcp://127.0.0.1:6000...\n",
      "You can open the visualizer by visiting the following URL:\n",
      "http://127.0.0.1:7000/static/\n",
      "Connected to meshcat-server.\n"
     ]
    },
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "f5c72b7c658b4414a24c2e920b35f8db",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "FloatSlider(value=0.0, description='FR_hip_joint', layout=Layout(width=\"'200'\"), max=0.872664625997, min=-1.04…"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "21e71afc68fa4b19bfdf41575009c320",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "FloatSlider(value=0.0, description='FR_thigh_joint', layout=Layout(width=\"'200'\"), max=3.92699081699, min=-0.5…"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "bdda40fccc07407088a0dacfd947d92c",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "FloatSlider(value=-0.610865238198, description='FR_calf_joint', layout=Layout(width=\"'200'\"), max=-0.610865238…"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "7cef4436eb8d4339922015f106d90982",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "FloatSlider(value=0.0, description='FL_hip_joint', layout=Layout(width=\"'200'\"), max=1.0471975512, min=-0.8726…"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "70f59e64330d4ae3ad0829175df376ad",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "FloatSlider(value=0.0, description='FL_thigh_joint', layout=Layout(width=\"'200'\"), max=3.92699081699, min=-0.5…"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "89ada802f20c420093d45e5ab5461587",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "FloatSlider(value=-0.610865238198, description='FL_calf_joint', layout=Layout(width=\"'200'\"), max=-0.610865238…"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "089ff374a16945d7ac32aa3d13cd159e",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "FloatSlider(value=0.0, description='RR_hip_joint', layout=Layout(width=\"'200'\"), max=0.872664625997, min=-1.04…"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "f89da830fecf4da8a356afad246a850b",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "FloatSlider(value=0.0, description='RR_thigh_joint', layout=Layout(width=\"'200'\"), max=3.92699081699, min=-0.5…"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "557ac7eff4424c318359906c4da4e5e8",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "FloatSlider(value=-0.610865238198, description='RR_calf_joint', layout=Layout(width=\"'200'\"), max=-0.610865238…"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "a554763183664532be263984c2079e14",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "FloatSlider(value=0.0, description='RL_hip_joint', layout=Layout(width=\"'200'\"), max=1.0471975512, min=-0.8726…"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "a78cfcc6a49842ba92c8dbcdeb77338c",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "FloatSlider(value=0.0, description='RL_thigh_joint', layout=Layout(width=\"'200'\"), max=3.92699081699, min=-0.5…"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "382a5f84aff244dfbaf872bed592b064",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "FloatSlider(value=-0.610865238198, description='RL_calf_joint', layout=Layout(width=\"'200'\"), max=-0.610865238…"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "#import sys\n",
    "#sys.path.append('/mnt/c/Users/ahl19/Documents/my_gitRepos/controller4quadruped/models/')\n",
    "\n",
    "builder = DiagramBuilder()\n",
    "\n",
    "plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=1e-4)\n",
    "\n",
    "Parser(plant, scene_graph).AddModelFromFile(\"/mnt/c/Users/ahl19/Documents/my_gitRepos/controller4quadruped/models/laikago/urdf/laikago1_obj.urdf\")\n",
    "#Parser(plant, scene_graph).AddModelFromFile(\"/mnt/c/Users/ahl19/Documents/my_gitRepos/controller4quadruped/models/laikago/urdf/laikago1.urdf\")\n",
    "\n",
    "\n",
    "\n",
    "plant.Finalize()\n",
    "\n",
    "meshcat = ConnectMeshcatVisualizer(builder, scene_graph, zmq_url=zmq_url)\n",
    "diagram = builder.Build()\n",
    "context = diagram.CreateDefaultContext()\n",
    "\n",
    "meshcat.load()\n",
    "MakeJointSlidersThatPublishOnCallback(plant, meshcat, context);"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "上面的导入中，惊奇的发现MeshCat还仅（官方）支持obj文件的模型。\n",
    "\n",
    "而unitree官方给的模型只有stl和dae模型文件。\n",
    "经过查找，在pybullet软件包给出的模型中，找到了带有obj模型文件的laikago模型。\n",
    "不过导入仍有问题。\n",
    "\n",
    "最近的一次尝试是用在线转换工具，autoconvert网站，将stl格式的模型文件转换为obj格式的模型文件。\n",
    "运行meshcat.load()是可以的（有了obj模型文件后，在urdf中无论使用stl或obj都可以）。\n",
    "但是在meshcat环境中只能看到机器人的四个足端（无论使用stl或obj都可以，都看不到其他部分。。）\n",
    "\n",
    "在多次尝试没有成功的情况下，放弃在MeshCat中查看的目标。\n",
    "改为在CoppeliaSim中进行查看。\n",
    "【注意】在coppelia和meshcat中查找模型文件的方式不同。coppelia（win）可以使用package，但meshcat需要用../\n",
    "\n",
    "\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 查看模型参数\n",
    "\n",
    "前面的导入虽然不能显示在MeshCat中，但模型的场景图已经构建在框图中。\n",
    "继续创建上下文，即可显示默认的参数。\n",
    "\n",
    "\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 13,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "::plant Context\n",
      "----------------\n",
      "Time: 0\n",
      "States:\n",
      "  1 discrete state groups with\n",
      "     37 states\n",
      "       1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0\n",
      "\n",
      "Parameters:\n",
      "  42 numeric parameter groups with\n",
      "     10 parameters\n",
      "       nan nan nan nan nan nan nan nan nan nan\n",
      "     10 parameters\n",
      "             13.733     0.002284     -4.1e-05     0.025165   0.00597435    0.0188927     0.018535   2.2185e-05  8.22026e-05 -4.45881e-06\n",
      "     10 parameters\n",
      "              1.096    -0.001568     0.008134     0.000864  0.000817012  0.000900282  0.000857629  1.72997e-05 -3.21489e-05 -9.59256e-06\n",
      "     10 parameters\n",
      "       0 0 0 0 0 0 0 0 0 0\n",
      "     10 parameters\n",
      "              1.528    -0.000482     -0.02001    -0.031996   0.00791374   0.00709733   0.00156723 -1.64432e-05  0.000148471  -0.00058454\n",
      "     10 parameters\n",
      "              0.241    -0.002196    -0.000381     -0.12338    0.0408741    0.0409393  0.000149258  1.46726e-07 -0.000283328 -2.56883e-05\n",
      "     10 parameters\n",
      "            0.06         0         0         0 0.0002809 0.0002809 0.0002809         0         0         0\n",
      "     10 parameters\n",
      "              1.096    -0.001568    -0.008134     0.000864  0.000817012  0.000900282  0.000857629 -1.72997e-05 -3.21489e-05  9.59256e-06\n",
      "     10 parameters\n",
      "       0 0 0 0 0 0 0 0 0 0\n",
      "     10 parameters\n",
      "             1.528   -0.000482     0.02001   -0.031996  0.00791374  0.00709733  0.00156723 1.64432e-05 0.000148471  0.00058454\n",
      "     10 parameters\n",
      "              0.241    -0.002196    -0.000381     -0.12338    0.0408741    0.0409393  0.000149258  1.46726e-07 -0.000283328 -2.56883e-05\n",
      "     10 parameters\n",
      "            0.06         0         0         0 0.0002809 0.0002809 0.0002809         0         0         0\n",
      "     10 parameters\n",
      "              1.096     0.001568     0.008134     0.000864  0.000817012  0.000900282  0.000857629 -1.72997e-05  3.21489e-05 -9.59256e-06\n",
      "     10 parameters\n",
      "       0 0 0 0 0 0 0 0 0 0\n",
      "     10 parameters\n",
      "              1.528    -0.000482     -0.02001    -0.031996   0.00791374   0.00709733   0.00156723 -1.64432e-05  0.000148471  -0.00058454\n",
      "     10 parameters\n",
      "              0.241    -0.002196    -0.000381     -0.12338    0.0408741    0.0409393  0.000149258  1.46726e-07 -0.000283328 -2.56883e-05\n",
      "     10 parameters\n",
      "            0.06         0         0         0 0.0002809 0.0002809 0.0002809         0         0         0\n",
      "     10 parameters\n",
      "             1.096    0.001568   -0.008134    0.000864 0.000817012 0.000900282 0.000857629 1.72997e-05 3.21489e-05 9.59256e-06\n",
      "     10 parameters\n",
      "       0 0 0 0 0 0 0 0 0 0\n",
      "     10 parameters\n",
      "             1.528   -0.000482     0.02001   -0.031996  0.00791374  0.00709733  0.00156723 1.64432e-05 0.000148471  0.00058454\n",
      "     10 parameters\n",
      "              0.241    -0.002196    -0.000381     -0.12338    0.0408741    0.0409393  0.000149258  1.46726e-07 -0.000283328 -2.56883e-05\n",
      "     10 parameters\n",
      "            0.06         0         0         0 0.0002809 0.0002809 0.0002809         0         0         0\n",
      "     12 parameters\n",
      "             1       0      -0       0       1       0       0       0       1 0.21935 -0.0875       0\n",
      "     12 parameters\n",
      "            1      0     -0      0      1      0      0      0      1      0 -0.059      0\n",
      "     12 parameters\n",
      "            1      0     -0      0      1      0      0      0      1      0 -0.037      0\n",
      "     12 parameters\n",
      "           1     0    -0     0     1     0     0     0     1     0     0 -0.25\n",
      "     12 parameters\n",
      "           1     0    -0     0     1     0     0     0     1     0     0 -0.25\n",
      "     12 parameters\n",
      "             1       0      -0       0       1       0       0       0       1 0.21935  0.0875       0\n",
      "     12 parameters\n",
      "           1     0    -0     0     1     0     0     0     1     0 0.059     0\n",
      "     12 parameters\n",
      "           1     0    -0     0     1     0     0     0     1     0 0.037     0\n",
      "     12 parameters\n",
      "           1     0    -0     0     1     0     0     0     1     0     0 -0.25\n",
      "     12 parameters\n",
      "           1     0    -0     0     1     0     0     0     1     0     0 -0.25\n",
      "     12 parameters\n",
      "              1        0       -0        0        1        0        0        0        1 -0.21935  -0.0875        0\n",
      "     12 parameters\n",
      "            1      0     -0      0      1      0      0      0      1      0 -0.059      0\n",
      "     12 parameters\n",
      "            1      0     -0      0      1      0      0      0      1      0 -0.037      0\n",
      "     12 parameters\n",
      "           1     0    -0     0     1     0     0     0     1     0     0 -0.25\n",
      "     12 parameters\n",
      "           1     0    -0     0     1     0     0     0     1     0     0 -0.25\n",
      "     12 parameters\n",
      "              1        0       -0        0        1        0        0        0        1 -0.21935   0.0875        0\n",
      "     12 parameters\n",
      "           1     0    -0     0     1     0     0     0     1     0 0.059     0\n",
      "     12 parameters\n",
      "           1     0    -0     0     1     0     0     0     1     0 0.037     0\n",
      "     12 parameters\n",
      "           1     0    -0     0     1     0     0     0     1     0     0 -0.25\n",
      "     12 parameters\n",
      "           1     0    -0     0     1     0     0     0     1     0     0 -0.25\n",
      "\n"
     ]
    }
   ],
   "source": [
    "context = plant.CreateDefaultContext()\n",
    "print(context)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 查看系统框图\n",
    "\n",
    "【对系统框图中各模块还不太熟悉】"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 14,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/svg+xml": [
       "<svg height=\"495pt\" viewBox=\"0.00 0.00 1154.00 495.00\" width=\"1154pt\" xmlns=\"http://www.w3.org/2000/svg\" xmlns:xlink=\"http://www.w3.org/1999/xlink\">\n",
       "<g class=\"graph\" id=\"graph0\" transform=\"scale(1 1) rotate(0) translate(4 491)\">\n",
       "<title>_94088071090496</title>\n",
       "<polygon fill=\"white\" points=\"-4,4 -4,-491 1150,-491 1150,4 -4,4\" stroke=\"transparent\"/>\n",
       "<g class=\"cluster\" id=\"clust1\">\n",
       "<title>cluster94088071090496diagram</title>\n",
       "<polygon fill=\"none\" points=\"8,-8 8,-479 1138,-479 1138,-8 8,-8\" stroke=\"black\"/>\n",
       "<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"573\" y=\"-463.8\">94088071090496</text>\n",
       "</g>\n",
       "<g class=\"cluster\" id=\"clust4\">\n",
       "<title>cluster94088071090496subsystems</title>\n",
       "<polygon fill=\"none\" points=\"16,-16 16,-448 1130,-448 1130,-16 16,-16\" stroke=\"white\"/>\n",
       "</g>\n",
       "<!-- 94088071146240 -->\n",
       "<g class=\"node\" id=\"node1\">\n",
       "<title>94088071146240</title>\n",
       "<polygon fill=\"none\" points=\"24,-25 24,-439 655,-439 655,-25 24,-25\" stroke=\"black\"/>\n",
       "<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"339.5\" y=\"-423.8\">plant</text>\n",
       "<polyline fill=\"none\" points=\"24,-416 655,-416 \" stroke=\"black\"/>\n",
       "<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"149.5\" y=\"-379.8\">geometry_query</text>\n",
       "<polyline fill=\"none\" points=\"24,-351 275,-351 \" stroke=\"black\"/>\n",
       "<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"149.5\" y=\"-314.8\">WorldModelInstance_actuation</text>\n",
       "<polyline fill=\"none\" points=\"24,-286 275,-286 \" stroke=\"black\"/>\n",
       "<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"149.5\" y=\"-249.8\">DefaultModelInstance_actuation</text>\n",
       "<polyline fill=\"none\" points=\"24,-221 275,-221 \" stroke=\"black\"/>\n",
       "<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"149.5\" y=\"-184.8\">laikago_description_actuation</text>\n",
       "<polyline fill=\"none\" points=\"24,-156 275,-156 \" stroke=\"black\"/>\n",
       "<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"149.5\" y=\"-119.8\">applied_generalized_force</text>\n",
       "<polyline fill=\"none\" points=\"24,-91 275,-91 \" stroke=\"black\"/>\n",
       "<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"149.5\" y=\"-54.3\">applied_spatial_force</text>\n",
       "<polyline fill=\"none\" points=\"275,-25 275,-416 \" stroke=\"black\"/>\n",
       "<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"465\" y=\"-400.8\">geometry_pose</text>\n",
       "<polyline fill=\"none\" points=\"275,-393 655,-393 \" stroke=\"black\"/>\n",
       "<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"465\" y=\"-377.8\">continuous_state</text>\n",
       "<polyline fill=\"none\" points=\"275,-370 655,-370 \" stroke=\"black\"/>\n",
       "<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"465\" y=\"-354.8\">body_poses</text>\n",
       "<polyline fill=\"none\" points=\"275,-347 655,-347 \" stroke=\"black\"/>\n",
       "<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"465\" y=\"-331.8\">spatial_velocities</text>\n",
       "<polyline fill=\"none\" points=\"275,-324 655,-324 \" stroke=\"black\"/>\n",
       "<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"465\" y=\"-308.8\">spatial_accelerations</text>\n",
       "<polyline fill=\"none\" points=\"275,-301 655,-301 \" stroke=\"black\"/>\n",
       "<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"465\" y=\"-285.8\">generalized_acceleration</text>\n",
       "<polyline fill=\"none\" points=\"275,-278 655,-278 \" stroke=\"black\"/>\n",
       "<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"465\" y=\"-262.8\">WorldModelInstance_continuous_state</text>\n",
       "<polyline fill=\"none\" points=\"275,-255 655,-255 \" stroke=\"black\"/>\n",
       "<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"465\" y=\"-239.8\">WorldModelInstance_generalized_acceleration</text>\n",
       "<polyline fill=\"none\" points=\"275,-232 655,-232 \" stroke=\"black\"/>\n",
       "<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"465\" y=\"-216.8\">DefaultModelInstance_continuous_state</text>\n",
       "<polyline fill=\"none\" points=\"275,-209 655,-209 \" stroke=\"black\"/>\n",
       "<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"465\" y=\"-193.8\">DefaultModelInstance_generalized_acceleration</text>\n",
       "<polyline fill=\"none\" points=\"275,-186 655,-186 \" stroke=\"black\"/>\n",
       "<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"465\" y=\"-170.8\">laikago_description_continuous_state</text>\n",
       "<polyline fill=\"none\" points=\"275,-163 655,-163 \" stroke=\"black\"/>\n",
       "<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"465\" y=\"-147.8\">laikago_description_generalized_acceleration</text>\n",
       "<polyline fill=\"none\" points=\"275,-140 655,-140 \" stroke=\"black\"/>\n",
       "<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"465\" y=\"-124.8\">WorldModelInstance_generalized_contact_forces</text>\n",
       "<polyline fill=\"none\" points=\"275,-117 655,-117 \" stroke=\"black\"/>\n",
       "<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"465\" y=\"-101.8\">DefaultModelInstance_generalized_contact_forces</text>\n",
       "<polyline fill=\"none\" points=\"275,-94 655,-94 \" stroke=\"black\"/>\n",
       "<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"465\" y=\"-78.8\">laikago_description_generalized_contact_forces</text>\n",
       "<polyline fill=\"none\" points=\"275,-71 655,-71 \" stroke=\"black\"/>\n",
       "<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"465\" y=\"-55.8\">reaction_forces</text>\n",
       "<polyline fill=\"none\" points=\"275,-48 655,-48 \" stroke=\"black\"/>\n",
       "<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"465\" y=\"-32.8\">contact_results</text>\n",
       "</g>\n",
       "<!-- 94088071148064 -->\n",
       "<g class=\"node\" id=\"node2\">\n",
       "<title>94088071148064</title>\n",
       "<polygon fill=\"none\" points=\"691,-370.5 691,-439.5 925,-439.5 925,-370.5 691,-370.5\" stroke=\"black\"/>\n",
       "<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"808\" y=\"-424.3\">scene_graph</text>\n",
       "<polyline fill=\"none\" points=\"691,-416.5 925,-416.5 \" stroke=\"black\"/>\n",
       "<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"738\" y=\"-389.8\">plant_pose</text>\n",
       "<polyline fill=\"none\" points=\"785,-370.5 785,-416.5 \" stroke=\"black\"/>\n",
       "<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"855\" y=\"-401.3\">lcm_visualization</text>\n",
       "<polyline fill=\"none\" points=\"785,-393.5 925,-393.5 \" stroke=\"black\"/>\n",
       "<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"855\" y=\"-378.3\">query</text>\n",
       "</g>\n",
       "<!-- 94088071146240&#45;&gt;94088071148064 -->\n",
       "<g class=\"edge\" id=\"edge3\">\n",
       "<title>94088071146240:y0-&gt;94088071148064:u0</title>\n",
       "<path d=\"M655,-405C667.78,-405 672.18,-398.11 680.9,-394.77\" fill=\"none\" stroke=\"black\"/>\n",
       "<polygon fill=\"black\" points=\"681.75,-398.17 691,-393 680.55,-391.28 681.75,-398.17\" stroke=\"black\"/>\n",
       "</g>\n",
       "<!-- 94088071148064&#45;&gt;94088071146240 -->\n",
       "<g class=\"edge\" id=\"edge2\">\n",
       "<title>94088071148064:y1-&gt;94088071146240:u0</title>\n",
       "<path d=\"M855,-369C855,-296.11 753.16,-331.93 691,-370 661.33,-388.17 684.72,-421.42 655,-439.5 595.05,-475.96 73.56,-488.67 23.5,-439.5 8.79,-425.05 0.73,-395.98 12.9,-386.82\" fill=\"none\" stroke=\"black\"/>\n",
       "<polygon fill=\"black\" points=\"13.89,-390.18 22.5,-384 11.92,-383.46 13.89,-390.18\" stroke=\"black\"/>\n",
       "</g>\n",
       "<!-- 94088069116848 -->\n",
       "<g class=\"node\" id=\"node3\">\n",
       "<title>94088069116848</title>\n",
       "<polygon fill=\"none\" points=\"961,-394 961,-440 1122,-440 1122,-394 961,-394\" stroke=\"black\"/>\n",
       "<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"1041.5\" y=\"-424.8\">meshcat_visualizer</text>\n",
       "<polyline fill=\"none\" points=\"961,-417 1122,-417 \" stroke=\"black\"/>\n",
       "<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"1031\" y=\"-401.8\">lcm_visualization</text>\n",
       "<polyline fill=\"none\" points=\"1101,-394 1101,-417 \" stroke=\"black\"/>\n",
       "<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"1111.5\" y=\"-401.8\"> </text>\n",
       "</g>\n",
       "<!-- 94088071148064&#45;&gt;94088069116848 -->\n",
       "<g class=\"edge\" id=\"edge1\">\n",
       "<title>94088071148064:y0-&gt;94088069116848:u0</title>\n",
       "<path d=\"M925,-405C937,-405 942.25,-405 950.88,-405\" fill=\"none\" stroke=\"black\"/>\n",
       "<polygon fill=\"black\" points=\"951,-408.5 961,-405 951,-401.5 951,-408.5\" stroke=\"black\"/>\n",
       "</g>\n",
       "</g>\n",
       "</svg>"
      ],
      "text/plain": [
       "<IPython.core.display.SVG object>"
      ]
     },
     "execution_count": 14,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "SVG(pydot.graph_from_dot_data(diagram.GetGraphvizString())[0].create_svg())"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 查看模型结构\n",
    "\n",
    "\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 15,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/svg+xml": [
       "<svg height=\"470pt\" viewBox=\"0.00 0.00 1681.00 470.00\" width=\"1681pt\" xmlns=\"http://www.w3.org/2000/svg\" xmlns:xlink=\"http://www.w3.org/1999/xlink\">\n",
       "<g class=\"graph\" id=\"graph0\" transform=\"scale(1 1) rotate(0) translate(4 466)\">\n",
       "<title>MultibodyPlant</title>\n",
       "<polygon fill=\"white\" points=\"-4,4 -4,-466 1677,-466 1677,4 -4,4\" stroke=\"transparent\"/>\n",
       "<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"836.5\" y=\"-446.8\">plant</text>\n",
       "<g class=\"cluster\" id=\"clust1\">\n",
       "<title>cluster0</title>\n",
       "<polygon fill=\"none\" points=\"8,-8 8,-83 173,-83 173,-8 8,-8\" stroke=\"black\"/>\n",
       "<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"90.5\" y=\"-15.8\">WorldModelInstance</text>\n",
       "</g>\n",
       "<g class=\"cluster\" id=\"clust3\">\n",
       "<title>cluster2</title>\n",
       "<polygon fill=\"none\" points=\"181,-8 181,-431 1665,-431 1665,-8 181,-8\" stroke=\"black\"/>\n",
       "<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"923\" y=\"-15.8\">laikago_description</text>\n",
       "</g>\n",
       "<!-- body0 -->\n",
       "<g class=\"node\" id=\"node1\">\n",
       "<title>body0</title>\n",
       "<ellipse cx=\"90\" cy=\"-57\" fill=\"none\" rx=\"61.99\" ry=\"18\" stroke=\"black\"/>\n",
       "<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"90\" y=\"-53.3\">WorldBody</text>\n",
       "</g>\n",
       "<!-- body1 -->\n",
       "<g class=\"node\" id=\"node2\">\n",
       "<title>body1</title>\n",
       "<ellipse cx=\"861\" cy=\"-405\" fill=\"none\" rx=\"36.29\" ry=\"18\" stroke=\"black\"/>\n",
       "<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"861\" y=\"-401.3\">trunk</text>\n",
       "</g>\n",
       "<!-- body2 -->\n",
       "<g class=\"node\" id=\"node3\">\n",
       "<title>body2</title>\n",
       "<ellipse cx=\"1312\" cy=\"-318\" fill=\"none\" rx=\"43.59\" ry=\"18\" stroke=\"black\"/>\n",
       "<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"1312\" y=\"-314.3\">FR_hip</text>\n",
       "</g>\n",
       "<!-- body2&#45;&gt;body1 -->\n",
       "<g class=\"edge\" id=\"edge1\">\n",
       "<title>body2-&gt;body1</title>\n",
       "<path d=\"M1277.21,-328.9C1237.51,-339.91 1170.5,-357.64 1112,-369 1040.68,-382.85 956.94,-393.39 906.47,-399.13\" fill=\"none\" stroke=\"black\"/>\n",
       "<polygon fill=\"black\" points=\"906.08,-395.66 896.53,-400.25 906.86,-402.61 906.08,-395.66\" stroke=\"black\"/>\n",
       "<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"1260.5\" y=\"-357.8\">FR_hip_joint [revolute]</text>\n",
       "</g>\n",
       "<!-- body3 -->\n",
       "<g class=\"node\" id=\"node4\">\n",
       "<title>body3</title>\n",
       "<ellipse cx=\"1515\" cy=\"-231\" fill=\"none\" rx=\"98.58\" ry=\"18\" stroke=\"black\"/>\n",
       "<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"1515\" y=\"-227.3\">FR_thigh_shoulder</text>\n",
       "</g>\n",
       "<!-- body3&#45;&gt;body2 -->\n",
       "<g class=\"edge\" id=\"edge2\">\n",
       "<title>body3-&gt;body2</title>\n",
       "<path d=\"M1511.67,-249.36C1508.66,-260.35 1503.08,-273.95 1493,-282 1473.52,-297.55 1410.77,-307.13 1364.59,-312.25\" fill=\"none\" stroke=\"black\"/>\n",
       "<polygon fill=\"black\" points=\"1364,-308.8 1354.43,-313.34 1364.74,-315.76 1364,-308.8\" stroke=\"black\"/>\n",
       "<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"1575\" y=\"-270.8\">FR_hip_fixed [weld]</text>\n",
       "</g>\n",
       "<!-- body4 -->\n",
       "<g class=\"node\" id=\"node5\">\n",
       "<title>body4</title>\n",
       "<ellipse cx=\"1314\" cy=\"-231\" fill=\"none\" rx=\"53.09\" ry=\"18\" stroke=\"black\"/>\n",
       "<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"1314\" y=\"-227.3\">FR_thigh</text>\n",
       "</g>\n",
       "<!-- body4&#45;&gt;body2 -->\n",
       "<g class=\"edge\" id=\"edge3\">\n",
       "<title>body4-&gt;body2</title>\n",
       "<path d=\"M1313.48,-249.09C1313.32,-254.79 1313.14,-261.17 1313,-267 1312.82,-274.35 1312.65,-282.3 1312.5,-289.66\" fill=\"none\" stroke=\"black\"/>\n",
       "<polygon fill=\"black\" points=\"1309,-289.81 1312.31,-299.87 1316,-289.94 1309,-289.81\" stroke=\"black\"/>\n",
       "<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"1403\" y=\"-270.8\">FR_thigh_joint [revolute]</text>\n",
       "</g>\n",
       "<!-- body5 -->\n",
       "<g class=\"node\" id=\"node6\">\n",
       "<title>body5</title>\n",
       "<ellipse cx=\"1314\" cy=\"-144\" fill=\"none\" rx=\"46.29\" ry=\"18\" stroke=\"black\"/>\n",
       "<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"1314\" y=\"-140.3\">FR_calf</text>\n",
       "</g>\n",
       "<!-- body5&#45;&gt;body4 -->\n",
       "<g class=\"edge\" id=\"edge4\">\n",
       "<title>body5-&gt;body4</title>\n",
       "<path d=\"M1314,-162.2C1314,-173.84 1314,-189.45 1314,-202.76\" fill=\"none\" stroke=\"black\"/>\n",
       "<polygon fill=\"black\" points=\"1310.5,-202.82 1314,-212.82 1317.5,-202.82 1310.5,-202.82\" stroke=\"black\"/>\n",
       "<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"1398.5\" y=\"-183.8\">FR_calf_joint [revolute]</text>\n",
       "</g>\n",
       "<!-- body6 -->\n",
       "<g class=\"node\" id=\"node7\">\n",
       "<title>body6</title>\n",
       "<ellipse cx=\"1314\" cy=\"-57\" fill=\"none\" rx=\"46.59\" ry=\"18\" stroke=\"black\"/>\n",
       "<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"1314\" y=\"-53.3\">FR_foot</text>\n",
       "</g>\n",
       "<!-- body6&#45;&gt;body5 -->\n",
       "<g class=\"edge\" id=\"edge5\">\n",
       "<title>body6-&gt;body5</title>\n",
       "<path d=\"M1314,-75.2C1314,-86.84 1314,-102.45 1314,-115.76\" fill=\"none\" stroke=\"black\"/>\n",
       "<polygon fill=\"black\" points=\"1310.5,-115.82 1314,-125.82 1317.5,-115.82 1310.5,-115.82\" stroke=\"black\"/>\n",
       "<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"1388\" y=\"-96.8\">FR_foot_fixed [weld]</text>\n",
       "</g>\n",
       "<!-- body7 -->\n",
       "<g class=\"node\" id=\"node8\">\n",
       "<title>body7</title>\n",
       "<ellipse cx=\"956\" cy=\"-318\" fill=\"none\" rx=\"42.79\" ry=\"18\" stroke=\"black\"/>\n",
       "<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"956\" y=\"-314.3\">FL_hip</text>\n",
       "</g>\n",
       "<!-- body7&#45;&gt;body1 -->\n",
       "<g class=\"edge\" id=\"edge6\">\n",
       "<title>body7-&gt;body1</title>\n",
       "<path d=\"M951.79,-336.02C948.45,-346.6 942.84,-359.87 934,-369 924.82,-378.48 912.55,-385.72 900.7,-391.08\" fill=\"none\" stroke=\"black\"/>\n",
       "<polygon fill=\"black\" points=\"899.23,-387.9 891.35,-394.98 901.93,-394.36 899.23,-387.9\" stroke=\"black\"/>\n",
       "<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"1026\" y=\"-357.8\">FL_hip_joint [revolute]</text>\n",
       "</g>\n",
       "<!-- body8 -->\n",
       "<g class=\"node\" id=\"node9\">\n",
       "<title>body8</title>\n",
       "<ellipse cx=\"1145\" cy=\"-231\" fill=\"none\" rx=\"98.28\" ry=\"18\" stroke=\"black\"/>\n",
       "<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"1145\" y=\"-227.3\">FL_thigh_shoulder</text>\n",
       "</g>\n",
       "<!-- body8&#45;&gt;body7 -->\n",
       "<g class=\"edge\" id=\"edge7\">\n",
       "<title>body8-&gt;body7</title>\n",
       "<path d=\"M1142.36,-249.28C1139.78,-260.23 1134.75,-273.84 1125,-282 1107.4,-296.74 1050.33,-306.37 1007.34,-311.73\" fill=\"none\" stroke=\"black\"/>\n",
       "<polygon fill=\"black\" points=\"1006.81,-308.27 997.3,-312.94 1007.65,-315.22 1006.81,-308.27\" stroke=\"black\"/>\n",
       "<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"1207\" y=\"-270.8\">FL_hip_fixed [weld]</text>\n",
       "</g>\n",
       "<!-- body9 -->\n",
       "<g class=\"node\" id=\"node10\">\n",
       "<title>body9</title>\n",
       "<ellipse cx=\"946\" cy=\"-231\" fill=\"none\" rx=\"52.79\" ry=\"18\" stroke=\"black\"/>\n",
       "<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"946\" y=\"-227.3\">FL_thigh</text>\n",
       "</g>\n",
       "<!-- body9&#45;&gt;body7 -->\n",
       "<g class=\"edge\" id=\"edge8\">\n",
       "<title>body9-&gt;body7</title>\n",
       "<path d=\"M944.83,-249.21C944.46,-258.89 944.48,-271.17 946,-282 946.38,-284.72 946.92,-287.53 947.56,-290.32\" fill=\"none\" stroke=\"black\"/>\n",
       "<polygon fill=\"black\" points=\"944.21,-291.34 950.15,-300.11 950.97,-289.55 944.21,-291.34\" stroke=\"black\"/>\n",
       "<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"1035.5\" y=\"-270.8\">FL_thigh_joint [revolute]</text>\n",
       "</g>\n",
       "<!-- body10 -->\n",
       "<g class=\"node\" id=\"node11\">\n",
       "<title>body10</title>\n",
       "<ellipse cx=\"946\" cy=\"-144\" fill=\"none\" rx=\"45.49\" ry=\"18\" stroke=\"black\"/>\n",
       "<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"946\" y=\"-140.3\">FL_calf</text>\n",
       "</g>\n",
       "<!-- body10&#45;&gt;body9 -->\n",
       "<g class=\"edge\" id=\"edge9\">\n",
       "<title>body10-&gt;body9</title>\n",
       "<path d=\"M946,-162.2C946,-173.84 946,-189.45 946,-202.76\" fill=\"none\" stroke=\"black\"/>\n",
       "<polygon fill=\"black\" points=\"942.5,-202.82 946,-212.82 949.5,-202.82 942.5,-202.82\" stroke=\"black\"/>\n",
       "<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"1030\" y=\"-183.8\">FL_calf_joint [revolute]</text>\n",
       "</g>\n",
       "<!-- body11 -->\n",
       "<g class=\"node\" id=\"node12\">\n",
       "<title>body11</title>\n",
       "<ellipse cx=\"946\" cy=\"-57\" fill=\"none\" rx=\"46.29\" ry=\"18\" stroke=\"black\"/>\n",
       "<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"946\" y=\"-53.3\">FL_foot</text>\n",
       "</g>\n",
       "<!-- body11&#45;&gt;body10 -->\n",
       "<g class=\"edge\" id=\"edge10\">\n",
       "<title>body11-&gt;body10</title>\n",
       "<path d=\"M946,-75.2C946,-86.84 946,-102.45 946,-115.76\" fill=\"none\" stroke=\"black\"/>\n",
       "<polygon fill=\"black\" points=\"942.5,-115.82 946,-125.82 949.5,-115.82 942.5,-115.82\" stroke=\"black\"/>\n",
       "<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"1019.5\" y=\"-96.8\">FL_foot_fixed [weld]</text>\n",
       "</g>\n",
       "<!-- body12 -->\n",
       "<g class=\"node\" id=\"node13\">\n",
       "<title>body12</title>\n",
       "<ellipse cx=\"767\" cy=\"-318\" fill=\"none\" rx=\"44.39\" ry=\"18\" stroke=\"black\"/>\n",
       "<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"767\" y=\"-314.3\">RR_hip</text>\n",
       "</g>\n",
       "<!-- body12&#45;&gt;body1 -->\n",
       "<g class=\"edge\" id=\"edge11\">\n",
       "<title>body12-&gt;body1</title>\n",
       "<path d=\"M762.82,-336.24C761.32,-346.64 761.38,-359.64 768,-369 779.01,-384.57 797.85,-393.22 815.7,-398.02\" fill=\"none\" stroke=\"black\"/>\n",
       "<polygon fill=\"black\" points=\"814.93,-401.44 825.47,-400.31 816.53,-394.62 814.93,-401.44\" stroke=\"black\"/>\n",
       "<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"851\" y=\"-357.8\">RR_hip_joint [revolute]</text>\n",
       "</g>\n",
       "<!-- body13 -->\n",
       "<g class=\"node\" id=\"node14\">\n",
       "<title>body13</title>\n",
       "<ellipse cx=\"777\" cy=\"-231\" fill=\"none\" rx=\"98.58\" ry=\"18\" stroke=\"black\"/>\n",
       "<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"777\" y=\"-227.3\">RR_thigh_shoulder</text>\n",
       "</g>\n",
       "<!-- body13&#45;&gt;body12 -->\n",
       "<g class=\"edge\" id=\"edge12\">\n",
       "<title>body13-&gt;body12</title>\n",
       "<path d=\"M774.98,-249.2C773.61,-260.84 771.77,-276.45 770.2,-289.76\" fill=\"none\" stroke=\"black\"/>\n",
       "<polygon fill=\"black\" points=\"766.71,-289.48 769.02,-299.82 773.67,-290.3 766.71,-289.48\" stroke=\"black\"/>\n",
       "<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"843.5\" y=\"-270.8\">RR_hip_fixed [weld]</text>\n",
       "</g>\n",
       "<!-- body14 -->\n",
       "<g class=\"node\" id=\"node15\">\n",
       "<title>body14</title>\n",
       "<ellipse cx=\"583\" cy=\"-231\" fill=\"none\" rx=\"53.89\" ry=\"18\" stroke=\"black\"/>\n",
       "<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"583\" y=\"-227.3\">RR_thigh</text>\n",
       "</g>\n",
       "<!-- body14&#45;&gt;body12 -->\n",
       "<g class=\"edge\" id=\"edge13\">\n",
       "<title>body14-&gt;body12</title>\n",
       "<path d=\"M579.16,-249.27C577.86,-259.95 578.32,-273.24 586,-282 602.46,-300.78 666.11,-309.67 713.3,-313.75\" fill=\"none\" stroke=\"black\"/>\n",
       "<polygon fill=\"black\" points=\"713.15,-317.25 723.4,-314.57 713.71,-310.27 713.15,-317.25\" stroke=\"black\"/>\n",
       "<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"676.5\" y=\"-270.8\">RR_thigh_joint [revolute]</text>\n",
       "</g>\n",
       "<!-- body15 -->\n",
       "<g class=\"node\" id=\"node16\">\n",
       "<title>body15</title>\n",
       "<ellipse cx=\"583\" cy=\"-144\" fill=\"none\" rx=\"46.29\" ry=\"18\" stroke=\"black\"/>\n",
       "<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"583\" y=\"-140.3\">RR_calf</text>\n",
       "</g>\n",
       "<!-- body15&#45;&gt;body14 -->\n",
       "<g class=\"edge\" id=\"edge14\">\n",
       "<title>body15-&gt;body14</title>\n",
       "<path d=\"M583,-162.2C583,-173.84 583,-189.45 583,-202.76\" fill=\"none\" stroke=\"black\"/>\n",
       "<polygon fill=\"black\" points=\"579.5,-202.82 583,-212.82 586.5,-202.82 579.5,-202.82\" stroke=\"black\"/>\n",
       "<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"667.5\" y=\"-183.8\">RR_calf_joint [revolute]</text>\n",
       "</g>\n",
       "<!-- body16 -->\n",
       "<g class=\"node\" id=\"node17\">\n",
       "<title>body16</title>\n",
       "<ellipse cx=\"583\" cy=\"-57\" fill=\"none\" rx=\"47.39\" ry=\"18\" stroke=\"black\"/>\n",
       "<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"583\" y=\"-53.3\">RR_foot</text>\n",
       "</g>\n",
       "<!-- body16&#45;&gt;body15 -->\n",
       "<g class=\"edge\" id=\"edge15\">\n",
       "<title>body16-&gt;body15</title>\n",
       "<path d=\"M583,-75.2C583,-86.84 583,-102.45 583,-115.76\" fill=\"none\" stroke=\"black\"/>\n",
       "<polygon fill=\"black\" points=\"579.5,-115.82 583,-125.82 586.5,-115.82 579.5,-115.82\" stroke=\"black\"/>\n",
       "<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"657\" y=\"-96.8\">RR_foot_fixed [weld]</text>\n",
       "</g>\n",
       "<!-- body17 -->\n",
       "<g class=\"node\" id=\"node18\">\n",
       "<title>body17</title>\n",
       "<ellipse cx=\"410\" cy=\"-318\" fill=\"none\" rx=\"43.59\" ry=\"18\" stroke=\"black\"/>\n",
       "<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"410\" y=\"-314.3\">RL_hip</text>\n",
       "</g>\n",
       "<!-- body17&#45;&gt;body1 -->\n",
       "<g class=\"edge\" id=\"edge16\">\n",
       "<title>body17-&gt;body1</title>\n",
       "<path d=\"M443.4,-329.61C479.83,-340.83 540.04,-358.36 593,-369 670.2,-384.51 761.35,-394.73 814.99,-399.92\" fill=\"none\" stroke=\"black\"/>\n",
       "<polygon fill=\"black\" points=\"814.91,-403.43 825.2,-400.9 815.57,-396.46 814.91,-403.43\" stroke=\"black\"/>\n",
       "<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"675.5\" y=\"-357.8\">RL_hip_joint [revolute]</text>\n",
       "</g>\n",
       "<!-- body18 -->\n",
       "<g class=\"node\" id=\"node19\">\n",
       "<title>body18</title>\n",
       "<ellipse cx=\"412\" cy=\"-231\" fill=\"none\" rx=\"98.58\" ry=\"18\" stroke=\"black\"/>\n",
       "<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"412\" y=\"-227.3\">RL_thigh_shoulder</text>\n",
       "</g>\n",
       "<!-- body18&#45;&gt;body17 -->\n",
       "<g class=\"edge\" id=\"edge17\">\n",
       "<title>body18-&gt;body17</title>\n",
       "<path d=\"M411.48,-249.09C411.32,-254.79 411.14,-261.17 411,-267 410.82,-274.35 410.65,-282.3 410.5,-289.66\" fill=\"none\" stroke=\"black\"/>\n",
       "<polygon fill=\"black\" points=\"407,-289.81 410.31,-299.87 414,-289.94 407,-289.81\" stroke=\"black\"/>\n",
       "<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"482\" y=\"-270.8\">RL_hip_fixed [weld]</text>\n",
       "</g>\n",
       "<!-- body19 -->\n",
       "<g class=\"node\" id=\"node20\">\n",
       "<title>body19</title>\n",
       "<ellipse cx=\"242\" cy=\"-231\" fill=\"none\" rx=\"53.09\" ry=\"18\" stroke=\"black\"/>\n",
       "<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"242\" y=\"-227.3\">RL_thigh</text>\n",
       "</g>\n",
       "<!-- body19&#45;&gt;body17 -->\n",
       "<g class=\"edge\" id=\"edge18\">\n",
       "<title>body19-&gt;body17</title>\n",
       "<path d=\"M229.26,-248.83C222.96,-259.33 218.21,-272.59 226,-282 242.36,-301.76 308.44,-310.46 356.75,-314.22\" fill=\"none\" stroke=\"black\"/>\n",
       "<polygon fill=\"black\" points=\"356.53,-317.72 366.76,-314.95 357.04,-310.74 356.53,-317.72\" stroke=\"black\"/>\n",
       "<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"316\" y=\"-270.8\">RL_thigh_joint [revolute]</text>\n",
       "</g>\n",
       "<!-- body20 -->\n",
       "<g class=\"node\" id=\"node21\">\n",
       "<title>body20</title>\n",
       "<ellipse cx=\"242\" cy=\"-144\" fill=\"none\" rx=\"46.29\" ry=\"18\" stroke=\"black\"/>\n",
       "<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"242\" y=\"-140.3\">RL_calf</text>\n",
       "</g>\n",
       "<!-- body20&#45;&gt;body19 -->\n",
       "<g class=\"edge\" id=\"edge19\">\n",
       "<title>body20-&gt;body19</title>\n",
       "<path d=\"M242,-162.2C242,-173.84 242,-189.45 242,-202.76\" fill=\"none\" stroke=\"black\"/>\n",
       "<polygon fill=\"black\" points=\"238.5,-202.82 242,-212.82 245.5,-202.82 238.5,-202.82\" stroke=\"black\"/>\n",
       "<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"326.5\" y=\"-183.8\">RL_calf_joint [revolute]</text>\n",
       "</g>\n",
       "<!-- body21 -->\n",
       "<g class=\"node\" id=\"node22\">\n",
       "<title>body21</title>\n",
       "<ellipse cx=\"242\" cy=\"-57\" fill=\"none\" rx=\"46.59\" ry=\"18\" stroke=\"black\"/>\n",
       "<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"242\" y=\"-53.3\">RL_foot</text>\n",
       "</g>\n",
       "<!-- body21&#45;&gt;body20 -->\n",
       "<g class=\"edge\" id=\"edge20\">\n",
       "<title>body21-&gt;body20</title>\n",
       "<path d=\"M242,-75.2C242,-86.84 242,-102.45 242,-115.76\" fill=\"none\" stroke=\"black\"/>\n",
       "<polygon fill=\"black\" points=\"238.5,-115.82 242,-125.82 245.5,-115.82 238.5,-115.82\" stroke=\"black\"/>\n",
       "<text font-family=\"Times,serif\" font-size=\"14.00\" text-anchor=\"middle\" x=\"316\" y=\"-96.8\">RL_foot_fixed [weld]</text>\n",
       "</g>\n",
       "</g>\n",
       "</svg>"
      ],
      "text/plain": [
       "<IPython.core.display.SVG object>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "display(SVG(pydot.graph_from_dot_data(plant.GetTopologyGraphvizString())[0].create_svg()))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.6.11"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}
